#ifndef myalgebra_cpp
#define myalgebra_cpp

//OpenCV
#include "cv.h"
#include "highgui.h"
#include "myalgebra.h"

inline double pointToLineDistance(double * point,double * pointAonLine, double * pointBonLine)
{
	double * x0 = point;
	double * x1 = pointAonLine;
	double * x2 = pointBonLine;
	double * tmp = minus3(x2,x1);
	double div = norm3(tmp);
	double * tmp2 = minus3(x0,x1);
	double * tmp3 = minus3(x0,x2);
	double * tmp4 = cross3(tmp2,tmp3);
	double top = norm3(tmp4);
	delete tmp;
	delete tmp2;
	delete tmp3;
	delete tmp4;	
	return top/div;
}

inline double pointToPointDistance(double * pointA,double * pointB)
{	
	return sqrt(pow(pointA[0]-pointB[0],2)+pow(pointA[1]-pointB[1],2)+pow(pointA[2]-pointB[2],2));
}

inline double * cross3(double * a, double * b)
{
	double * ret = new double[3];
	ret[0] = a[1]*b[2]-a[2]*b[1];
	ret[1] = a[2]*b[0]-a[0]*b[2];
	ret[2] = a[0]*b[1]-a[1]*b[0];
	return ret;
}


inline double norm3(double * a)
{
	return sqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);
}

inline double * minus3(double * a, double * b)
{
	double * ret = new double[3];
	ret[0] = a[0]-b[0];
	ret[1] = a[1]-b[1];
	ret[2] = a[2]-b[2];
	return ret;
}

void matmul(double ** A, int rows_a, int cols_a, double ** B, int rows_b, int cols_b, double ** C)
{
	for (int i = 0 ; i < rows_a; i++)
	{
		for (int j = 0;  j < cols_b; j++)
		{
		    C[i][j] = 0;
		    for (int k = 0; k < cols_a; k++)
		    {
		        C[i][j] += A[i][k] * B[k][j];
		    }
		}
	}
}

void matmul_verbose(double ** A, int rows_a, int cols_a, double ** B, int rows_b, int cols_b, double ** C)
{
printf("A = \n");
	for(int i = 0; i < rows_a; i++)
	{
	    printf("| ");	
		for(int j = 0; j < cols_a-1; j++)
		{
			if(A[i][j] < 0)
			{
				printf("%f, ",A[i][j]);
			}else{
				printf(" %f, ",A[i][j]);
			}
		}
		if(A[i][cols_a-1] < 0)
		{
			printf("%f ",A[i][cols_a-1]);
		}else{
			printf(" %f ",A[i][cols_a-1]);
		}
		printf("|\n");	
	}
	
	printf("B = \n");
	for(int i = 0; i < rows_b; i++)
	{
	    printf("| ");	
		for(int j = 0; j < cols_b-1; j++)
		{
			if(B[i][j] < 0)
			{
				printf("%f, ",B[i][j]);
			}else{
				printf(" %f, ",B[i][j]);
			}
		}
		if(B[i][cols_b-1] < 0)
		{
			printf("%f ",B[i][cols_b-1]);
		}else{
			printf(" %f ",B[i][cols_b-1]);
		}
		printf("|\n");	
	}
	
	for (int i = 0 ; i < rows_a; i++)
	{
		for (int j = 0;  j < cols_b; j++)
		{
		    C[i][j] = 0;
		    for (int k = 0; k < cols_a; k++)
		    {
		        C[i][j] += A[i][k] * B[k][j];
		    }
		}
	}

printf("A * B = \n");
	for(int i = 0; i < rows_a; i++)
	{
	    printf("| ");	
		for(int j = 0; j < cols_b-1; j++)
		{
			if(C[i][j] < 0)
			{
				printf("%f, ",C[i][j]);
			}else{
				printf(" %f, ",C[i][j]);
			}
		}
		if(C[i][cols_b-1] < 0)
		{
			printf("%f ",C[i][cols_b-1]);
		}else{
			printf(" %f ",C[i][cols_b-1]);
		}
		printf("|\n");	
	}
}
#endif
